# can_controller_node.py
import rclpy
from rclpy.node import Node
import serial
import time
import threading

# ROS 2 消息类型
from my_vision_interfaces.msg import TargetPose
from std_msgs.msg import String # 用来发布接收到的CAN消息

class CanControllerNode(Node):
    def __init__(self):
        super().__init__('python_can_controller_node')

        # --- 1. 参数化串口配置 (更符合ROS要求) ---
        self.declare_parameter('port', "/dev/serial/by-id/usb-1a86_USB_Single_Serial_5972089810-if00")
        self.declare_parameter('baudrate', 4000000)
        port_path = self.get_parameter('port').get_parameter_value().string_value
        baud_rate = self.get_parameter('baudrate').get_parameter_value().integer_value

        # --- 2. 打开串口 ---
        self.get_logger().info(f'尝试连接串口: {port_path} @ {baud_rate}bps')
        try:
            self.serial_handler = serial.Serial(port_path, baud_rate, timeout=1.0)
            time.sleep(1)
            self.get_logger().info('串口连接成功！')
        except serial.SerialException as e:
            self.get_logger().error(f'无法打开串口: {e}')
            # 如果串口失败，节点就没意义了，直接关闭
            rclpy.shutdown()
            return

        # --- 3. ROS部分：订阅与发布 ---
        # 订阅来自vision_node的位姿话题，触发发送逻辑
        self.subscription = self.create_subscription(
            TargetPose,
            '/target_pose',
            self.pose_callback,
            10)

        # 创建一个发布者，用于将接收到的CAN消息转发到ROS话题中
        self.can_rx_publisher = self.create_publisher(String, '/can_received_data', 10)

        # --- 4. 启动接收线程 ---
        self.shutdown_event = threading.Event()
        self.receiver_thread = threading.Thread(target=self.receiver_task)
        self.receiver_thread.start()

        self.get_logger().info('CAN控制器已启动。')

    def pose_callback(self, msg: TargetPose):
    
        target_x = msg.tvec.x * 1000
        target_y = msg.tvec.y * 1000
        target_z = msg.tvec.z * 1000

        # 打包并发送ID 0x101: X, Y 坐标
        id_xy = 0x101
        x_int = int(target_x)
        y_int = int(target_y)
        # 使用Python的 to_bytes 方法
        # (2, byteorder='big', signed=True) 表示转成2个字节, 大端序, 有符号整数
        payload_xy = bytearray()
        payload_xy.extend((id_xy).to_bytes(2, byteorder='big'))
        payload_xy.extend((x_int).to_bytes(2, byteorder='big', signed=True))
        payload_xy.extend((y_int).to_bytes(2, byteorder='big', signed=True))
        self.serial_handler.write(payload_xy)

        # 打包并发送ID 0x102: Z 坐标
        id_z = 0x102
        z_int = int(target_z)
        payload_z = bytearray()
        payload_z.extend((id_z).to_bytes(2, byteorder='big'))
        payload_z.extend((z_int).to_bytes(2, byteorder='big', signed=True))
        self.serial_handler.write(payload_z)

        self.get_logger().info(f'已发送目标坐标 -> X: {x_int}, Y: {y_int}, Z: {z_int}')

    def receiver_task(self):
        """
        接收逻辑的核心，运行在一个独立的线程中。
        保留了经过验证的帧头对齐和解析逻辑。
        """
        self.get_logger().info('CAN接收线程已启动。')
        aligned = False
        while rclpy.ok() and not self.shutdown_event.is_set():
            try:
                # 寻找帧头
                if not aligned:
                    header = self.serial_handler.read_until(b'\xff\xff\xff\xff\xff\xff\xff\xff\xff')
                    if len(header) == 9:
                        aligned = True
                    else:
                        continue

                # 读取剩下的数据 (20字节帧 - 9字节帧头 = 11字节)
                frame_data = self.serial_handler.read(11)
                aligned = False # 读完一帧后重新寻找帧头

                if len(frame_data) == 11:
                    # 解析数据 (根据固件协议)
                    # frame_data[0] = length and status
                    can_id = int.from_bytes(frame_data[1:3], byteorder='big')
                    can_payload = frame_data[3:11]

                    # 将收到的数据发布成ROS消息
                    log_msg = String()
                    log_msg.data = f"收到CAN消息 -> ID: {hex(can_id)}, Payload: {can_payload.hex(' ')}"
                    self.can_rx_publisher.publish(log_msg)
                    self.get_logger().info(log_msg.data) # 也在本地打印一份

            except serial.SerialException:
                self.get_logger().error("串口断开！")
                break
            except Exception as e:
                self.get_logger().error(f"接收线程发生错误: {e}")
                time.sleep(1)

    def destroy_node(self):
        """确保在关闭时能安全停止线程"""
        self.get_logger().info("正在关闭CAN控制器节点...")
        self.shutdown_event.set()
        self.receiver_thread.join()
        if self.serial_handler.is_open:
            self.serial_handler.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = CanControllerNode()
    if rclpy.ok():
        try:
            rclpy.spin(node)
        except KeyboardInterrupt:
            pass
        finally:
            # destroy_node 会被自动调用
            pass

if __name__ == '__main__':
    main()
